#include "rm_serial_driver/SerialEncoder.hpp"


void encode_ctr_data(const ctr_data_raw_float *input, ctr_data *output) {

    output->chassis_x = LIMIT_INSIDE(MAP(input->chassis_x, -1.5, 1.5, -INT8_MAX, INT8_MAX), -INT8_MAX, INT8_MAX);
    output->chassis_y = LIMIT_INSIDE(MAP(input->chassis_y, -1.5, 1.5, -INT8_MAX, INT8_MAX), -INT8_MAX, INT8_MAX);
    output->chassis_yaw = LIMIT_INSIDE(MAP(input->chassis_yaw, -5, 5, -INT8_MAX, INT8_MAX), -INT8_MAX, INT8_MAX);
    output->down_gimbal_shoot = input->down_gimbal_shoot ? 1 : 0;
    output->up_gimbal_shoot = input->up_gimbal_shoot ? 1 : 0;
    output->down_gimbal_delta_yaw = LIMIT_INSIDE(MAP(input->down_gimbal_delta_yaw, -1.0, 1.0, -INT14_MAX, INT14_MAX), -INT14_MAX, INT14_MAX);
    output->up_gimbal_delta_yaw = LIMIT_INSIDE(MAP(input->up_gimbal_delta_yaw, -1.0, 1.0, -INT8_MAX, INT8_MAX), -INT8_MAX, INT8_MAX);
    output->down_gimbal_delta_pitch = LIMIT_INSIDE(MAP(input->down_gimbal_delta_pitch, -0.3, 0.3, -INT8_MAX, INT8_MAX), -INT8_MAX, INT8_MAX);
    output->up_gimbal_delta_pitch = LIMIT_INSIDE(MAP(input->up_gimbal_delta_pitch, -0.3, 0.3, -INT8_MAX, INT8_MAX), -INT8_MAX, INT8_MAX);

    // swap_bytes((uint8_t*)output, C_DATA_INT16_OFFSET, C_DATA_INT16_OFFSET + 1);
    // high, low, for the 16 bit int

}

void decode_ctr_data(const ctr_data *input, ctr_data_raw_float *output) {
    // swap_bytes((uint8_t*)input, C_DATA_INT16_OFFSET, C_DATA_INT16_OFFSET + 1);

    output->chassis_x = MAP(input->chassis_x, -INT8_MAX, INT8_MAX, -1.5, 1.5);
    output->chassis_y = MAP(input->chassis_y, -INT8_MAX, INT8_MAX, -1.5, 1.5);
    output->chassis_yaw = MAP(input->chassis_yaw, -INT8_MAX, INT8_MAX, -5, 5);
    output->down_gimbal_shoot = input->down_gimbal_shoot;
    output->up_gimbal_shoot = input->up_gimbal_shoot;
    output->down_gimbal_delta_yaw = MAP(input->down_gimbal_delta_yaw, -INT14_MAX, INT14_MAX, -1.0, 1.0);
    output->up_gimbal_delta_yaw = MAP(input->up_gimbal_delta_yaw, -INT8_MAX, INT8_MAX, -1.0, 1.0);
    output->down_gimbal_delta_pitch = MAP(input->down_gimbal_delta_pitch, -INT8_MAX, INT8_MAX, -0.3, 0.3);
    output->up_gimbal_delta_pitch = MAP(input->up_gimbal_delta_pitch, -INT8_MAX, INT8_MAX, -0.3, 0.3);

}

// void swap_bytes(uint8_t *data, int index1, int index2) {
//     uint8_t temp = data[index1];
//     data[index1] = data[index2];
//     data[index2] = temp;
// }